#ifndef __BSPLINES__
#define __BSPLINES__

#include "vector.h"
#include <cmath>
#include <vector>
#include <boost/array.hpp>

namespace math {
namespace spline {
template<class T>
std::vector<T> GenerateOpenKnotvector(size_t p, size_t n) {
	std::vector<T> U;
	for(size_t i = 0; i <= p; i++)
		U.push_back((T)0.0);

	T inv = (T)(1.0 / (n - p));
	for(size_t i = 1; i <= n - (p + 1); i++)
		U.push_back(i * inv);

	for(size_t i = 0; i <= p; i++)
		U.push_back((T)1.0);
	return U;
}

template<class T>
std::vector<T> GeneratePeriodicKnotvector(size_t p, size_t n) {
	std::vector<T> U;
	for(int i = -p; i <= (int)(n + p); i++)
		U.push_back((T)i / n);
	return U;
}

template<class T>
std::vector<T> GenerateFloatingKnotvector(size_t p, size_t n) {
	std::vector<T> U;
	for(size_t i = 0; i < n + p + 1; i++)
		U.push_back((T)i / (n + p));
	return U;
}

template<class T>
T Node(const std::vector<T>& U, size_t k, size_t p) {
	T f = (T)0.0;
	size_t n = U.size();
	for(size_t i = 1; i <= p; i++)	
		f += U[(k + i) % n];
	f = (T)(f / p);

	if(f < (T)0.0)
		f += (T)1.0;
	else if(f > (T)1.0)
		f -= (T)1.0;

	return f;
}
	
template<class T>
size_t FindSpan(size_t n, size_t p, T u, const std::vector<T>& U) {
	//Return the knot span index
	if(u == U[n + 1]) return n;
	size_t low = p;
	size_t high = n + 1;
	size_t mid = (low + high) / 2;
	while(u < U[mid] || u >= U[mid + 1]) {
		if(u < U[mid])
			high = mid;
		else
			low = mid;
		mid = (low + high) / 2;
	}
	return mid;
}

template<class T>
void FindSpanMult(size_t n, size_t p, T u, const std::vector<T>& U, size_t& k, size_t& s) {
	k = FindSpan(n, p, u, U);
	s = k;
	while(s < U.size() && u == U[s])
		s--;
	s = k - s;
}

template<class T>
void BasisFuns(size_t i, T u, size_t p, const std::vector<T>& U, std::vector<T>& N) {
	if(N.empty())
		N.resize(p + 1);

	N[0] = 1.0;
	std::vector<T> left(p + 1);
	std::vector<T> right(p + 1);
	for(size_t j = 1; j <= p; j++) {
		left[j] = u - U[i + 1 - j];
		right[j] = U[i + j] - u;
		T saved = (T)0;
		for(size_t r = 0; r < j; r++) {
			T temp = N[r] / (right[r + 1] + left[j - r]);
			N[r] = saved + right[r + 1] * temp;
			saved = left[j - r] * temp;
		}
		N[j] = saved;
	}
}

template<class T>
T BasisFun(T u, size_t i, size_t p, const std::vector<T>& U) {
	T Nip;
	T saved, Uleft, Uright, temp;
  
	if((i == 0 && u == U[0]) || (i == U.size() - p - 2 && u == U[U.size() - 1])){
		Nip = (T)1.0 ;
		return Nip ;
	}
	
	if(u < U[i] || u >= U[i + p + 1]) {
		Nip = (T)0.0 ;
		return Nip;
	}

	std::vector<T> N(p + 1);

	size_t j;
	for(j = p; j >= 0; --j){
		if(u >= U[i + j] && u < U[i + j + 1]) 
			N[j] = (T)1.0;
		else
			N[j] = (T)0.0;

		if(j == 0)
			break;
	}
	
	for(size_t k = 1; k <= p; k++){
		if(N[0] == (T)0.0)
			saved = (T)0.0 ;
		else
			saved = ((u - U[i]) * N[0]) / (U[i + k] - U[i]) ;
		
		for(j = 0;j < p - k + 1; j++){
			Uleft = U[i + j + 1] ;
			Uright = U[i + j + k + 1] ;
			if(N[j + 1] == (T)0.0) {
				N[j] = saved ;
				saved = (T)0.0 ;
			} else {
				temp = N[j + 1] / (Uright - Uleft) ;
				N[j] = saved + (Uright - u) * temp ;
				saved = (u - Uleft) * temp ;
			}
		}
	}
	Nip = N[0] ;

	return Nip;  
}

template<class T>
std::vector< std::vector<T> > DersBasisFuns(size_t i, T u, size_t p, size_t n, const std::vector<T>& U) {
	std::vector< std::vector<T> > ders(n + 1);
	size_t s = p + 1;
	for(size_t j = 0; j < n + 1; j++)
		ders[j].resize(s);
	
	std::vector< std::vector<T> > ndu(s);
	for(size_t j = 0; j < s; j++)
		ndu[j].resize(s);

	std::vector< std::vector<T> > a(2);
	a[0].resize(s);
	a[1].resize(s);

	std::vector<T> left(p + 1);
	std::vector<T> right(p + 1);

	ndu[0][0] = (T)1.0;
	for(size_t j = 1; j <= p; j++) {
		left[j] = u - U[i + 1 - j];
		right[j] = U[i + j] - u;
		T saved = (T)0.0;
		for(size_t r = 0; r < j; r++) {
			ndu[j][r] = right[r + 1] + left[j - r];
			T temp = ndu[r][j - 1] / ndu[j][r];

			ndu[r][j] = saved + right[r + 1] * temp;
			saved = left[j - r] * temp;
		}
		ndu[j][j] = saved;
	}
	
	for(size_t j = 0; j <= p; j++) {
		ders[0][j] = ndu[j][p];	 
	}
	
	for(size_t r = 0; r <= p; r++) {
		size_t s1 = 0;
		size_t s2 = 1;
		a[0][0] = 1.0;
		for(size_t k = 1; k <= n; k++) {
			T d = (T)0.0;
			int rk = (int)(r - k);
			int pk = (int)(p - k);
			if(r >= k) {
				a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
				d = a[s2][0] * ndu[rk][pk];
			}

			int j1, j2;
			if(rk >= -1)
				j1 = 1;
			else
				j1 = -rk;

			if((int)(r - 1) <= pk)
				j2 = (int)(k - 1);
			else
				j2 = (int)(p - r);
			for(size_t j = j1; j <= (size_t)j2; j++) {
				a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][rk + j];
				d += a[s2][j] * ndu[rk + j][pk];
			}
			if((int)r <= pk) {
				a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
				d += a[s2][k] * ndu[r][pk];
			}
			ders[k][r] = d;
			int j = (int)s1;
			s1 = s2;
			s2 = (size_t)j;
		}
	}

	size_t r = p;
	for(size_t k = 1; k <= n; k++) {
		for(size_t j = 0; j <= p; j++)
			ders[k][j] *= r;
		r *= (p - k);
	}
	
	return ders;
}

template<class T>
T BasisFunDer(T u, size_t i, size_t p, const std::vector<T>& U) {
	size_t n = U.size() - (p + 1);
	T der;

	T c1;
	if(std::abs(U[i + p] - U[i]) < 0.00000000000001)
		c1 = (T)0.0;
	else if(i == n - 1 && std::abs(1.0 - u) < 0.000001)
		c1 = (T)1.0;
	else 
		c1 = spline::BasisFun(u, i, p - 1, U) / (U[i + p] - U[i]);

	T c2;
	if(std::abs(U[i + 1 + p] - U[i + 1]) < 0.00000000000001)
		c2 = (T)0.0;
	else if(i == n - 2 && std::abs(1.0 - u) < 0.000001)
		c2 = (T)1.0;
	else
		c2 = spline::BasisFun(u, i + 1, p - 1, U) / (U[i + 1 + p] - U[i + 1]);

	T d = c1 - c2;
	if(d != (T)0.0)
		der = p * d;
	else
		der = (T)0.0;

	return der;
}

template<class T, class S>
T SurfaceEvaluate(const boost::array<S, 2>& param, const std::vector< std::vector<T> >& Pw, size_t p, size_t q, size_t n, size_t m, const std::vector<S>& U, const std::vector<S>& V) {
	const S& u = param[0];
	const S& v = param[1];

	std::vector<S> Nu(p + 1);
	std::vector<S> Nv(q + 1);

	size_t uspan = spline::FindSpan<S>(n, p, u, U);
	spline::BasisFuns<S>(uspan, u, p, U, Nu);
	size_t vspan = spline::FindSpan<S>(m, q, v, V);
	spline::BasisFuns<S>(vspan, v, q, V, Nv);

	std::vector< T > temp(q + 1);
	for(size_t l = 0; l <= q; l++) {
		for(size_t k = 0; k <= p; k++) {
			temp[l] = temp[l] + Nu[k] * Pw[uspan - p + k][vspan - q + l];
		}
	}

	T Sw;
	for(size_t l = 0; l <= q; l++)
		Sw = Sw + Nv[l] * temp[l];

	return Sw;
}

template<class T, class S>
T VolumeEvaluate(const boost::array<S, 3>& param, const std::vector< std::vector< std::vector<T> > >& Pw, size_t p, size_t q, size_t r, size_t n, size_t m, size_t o, const std::vector<S>& U, const std::vector<S>& V, const std::vector<S>& W) {
	// u, v, w
	const S& u = param[0];
	const S& v = param[1];
	const S& w = param[2];
	
	// define the vector to store the non zero basis functions
	std::vector<S> Nu(p + 1);
	std::vector<S> Nv(q + 1);
	std::vector<S> Nw(r + 1);

	size_t uspan = spline::FindSpan<S>(n, p, u, U);
	spline::BasisFuns<S>(uspan, u, p, U, Nu);
	size_t vspan = spline::FindSpan<S>(m, q, v, V);
	spline::BasisFuns<S>(vspan, v, q, V, Nv);
	size_t wspan = spline::FindSpan<S>(o, r, w, W);
	spline::BasisFuns<S>(wspan, w, r, W, Nw);
	std::vector< std::vector<T> > temp(r+1);

	for(size_t i = 0; i <= r; i++)
		temp[i].resize(q + 1);

	for(size_t j = 0; j <= r; j++) {
		for(size_t l = 0;l <= q; l++) {
			for(size_t k = 0; k <= p; k++) {
				temp[j][l] = temp[j][l] + Nu[k] * Pw[uspan - p + k][vspan - q + l][wspan - r + j];
			}
		}
	}

	std::vector<T> temp1(q+1);
	
	for(size_t j = 0; j <= r; j++) {
		for(size_t l = 0; l <= q; l++) {
			temp1[j] = temp1[j] + Nv[l] * temp[j][l];
		}
	}

	T Sw;
	for(size_t j = 0; j <= r; j++)
		Sw = Sw + Nw[j] * temp1[j];

	return Sw;
}

template<class type, size_t degree>
math::Vec<type, degree + 1> B(type t) {
	std::vector<type> U(2 * (degree + 1));
	for(size_t i = 0; i <= degree; i++) {
		U[i] = (type)0;
		U[i + degree] = (type)1;
	}
	
	std::vector<type> N;
	BasisFuns(degree, t, degree, U, N);

	math::Vec<type, degree + 1> v;

	for(size_t i = 0; i <= degree; i++)
		v[i] = N[i];

	return v;
}

template<class type>
math::Vec<type, 4> B(type t) {
	std::cout << "Hello." << std::endl;
	type b = -(type)1 + t;
	type b2 = b * b;
	type t2 = t * t;

	return V4n(-(b * b2),
			   (type)3 * b2 * t,
			   -(type)3 * b * t2,
			   t * t2);
}

template<class type, size_t degree>
math::Vec<type, degree + 1> Bd(type t) {
	std::vector<type> U(2 * (degree + 1));
	for(size_t i = 0; i <= degree; i++) {
		U[i] = (type)0;
		U[i + degree] = (type)1;
	}

	math::Vec<type, degree + 1> v;
	for(size_t i = 0; i <= degree; i++)
		v[i] = BasisFunDer(t, i, degree, U);

	return v;
}

template<class type>
math::Vec<type, 4> Bd(type t) {
	std::cout << "HelloD." << std::endl;
	return V4n(-(type)3 * ((-(type)1 + t) * (-(type)1 + t)),
				(type)3 * ((type)1 + t * (-(type)4 + (type)3 * t)),
				(type)3 * ((type)2 - (type)3 * t) * t,
				(type)3 * t * t);
}

template<class type, size_t degree>
std::pair< math::Vec<type, degree + 1>, math::Vec<type, degree + 1> > Bnd(type t) {
	std::pair< math::Vec<type, degree + 1>, math::Vec<type, degree + 1> > v;
	v.first = B<type, degree>(t);
	v.second = Bd<type, degree>(t);
	return v;
}

template<class type>
std::pair< math::Vec<type, 4>, math::Vec<type, 4> > Bnd3(type t) {
	type b = -(type)1 + t;
	type b2 = b * b;
	type t2 = t * t;

	std::pair< math::Vec<type, 4>, math::Vec<type, 4> > v(
		V4n(-(b * b2), (type)3 * b2 * t, -(type)3 * b * t2, t * t2),
		V4n(-(type)3 * b2, (type)3 * ((type)1 + t * (-(type)4 + (type)3 * t)), (type)3 * ((type)2 - (type)3 * t) * t, (type)3 * t2)
	);

	return v;
}

template<class type>
std::pair< math::Vec<type, 6>, math::Vec<type, 6> > Bnd5(type t) {
	type b = -(type)1 + t;
	type b2 = b * b;
	type b3 = b2 * b;
	type b4 = b2 * b2;

	type t2 = t * t;
	type t3 = t2 * t;
	type t4 = t2 * t2;
	type fivet = (type)5 * t;

	std::pair< math::Vec<type, 6>, math::Vec<type, 6> > v(
		V6n(-b4 * b, 
			(type)5 * b4 * t, 
			-(type)10 * b3 * t2, 
			(type)10 * b2 * t3, 
			-(type)5 * b * t4, 
			t4 * t),
		V6n(-(type)5 * b4, 
			(type)5 * b3 * (fivet - (type)1), 
			-(type)10 * b2 * t * (fivet - (type)2), 
			(type)10 * b * t2 * (fivet - (type)3), 
			(type)5 * ((type)4 - fivet) * t3, 
			(type)5 * t4)
	);

	return v;
}

template<class type>
std::pair< math::Vec<type, 5>, math::Vec<type, 5> > Bnd4(type t) {
	type b = -(type)1 + t;
	type b2 = b * b;
	type b3 = b2 * b;
	type b4 = b2 * b2;

	type t2 = t * t;
	type t3 = t2 * t;
	type t4 = t2 * t2;
	type threet = (type)3 * t;
	type fourt = (type)4 * t;

	std::pair< math::Vec<type, 5>, math::Vec<type, 5> > v(
		V5n(b4, 
			-(type)4 * b3, 
			(type)6 * b2 * t2, 
			-(type)4 * b * t3,
			t4),
		V5n((type)4 * b3, 
			-(type)4 * b2 * (fourt - (type)1), 
			(type)12 * t * ((type)1 - threet + (type)2 * t2), 
			(type)4 * ((type)3 - fourt) * t2, 
			(type)4 * t3)
	);

	return v;
}

template<class type>
std::pair< math::Vec<type, 3>, math::Vec<type, 3> > Bnd2(type t) {
	type b = -(type)1 + t;

	std::pair< math::Vec<type, 3>, math::Vec<type, 3> > v(
		V3n(b * b, -(type)2 * b * t, t * t),
		V3n((type)2 * b, (type)2 - (type)4 * t, (type)2 * t) 
	);

	return v;
}

template<class type>
std::pair< math::Vec<type, 2>, math::Vec<type, 2> > Bnd1(type t) {
	std::pair< math::Vec<type, 2>, math::Vec<type, 2> > v(
		V2n((type)1 - t, t),
		V2n(-(type)1, (type)1) 
	);

	return v;
}

}

namespace power {

template<class T>
T BasisFun(T x, int i) {
	return pow(x, i);
}

template<class T>
T BasisFunDer(T x, int i) {
	return i * pow(x, i - 1);
}

}

}
#endif //__BSPLINES__
